;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;           unconstrained optimization(无约束最优化问题)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; Consider the optimization problem:
;;                          min f(x), where x is in R^n.

;; This problem is generally solved by iteration methods. i.e. x0 -> x1 = x0 + s * d
;; We can classify iteration methods into two classes: Linear-search methods and Trust-region methods.
;; In this note, we give Line-Search method

;; IDEA of line-search method: start at x0, to get next point, x1,
;; (a) find-direction, d;
;; (b) get-step, s;
;; (c) get x1 = x0 + s * d.
;;NOTE: find-direction, get-step need to be defined.


#!r6rs

(library-directories '("." "/home/cl/gits/libs-for-chezscheme"))
(import (linear-algebra)
        (stream)
        (range)
        (gnuplot))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; (A) get-step
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; --- line-search-conditions
;; Given direction d at x0, and ask whether the step, s, is allowed.
;; Below, current-point: x0, test-next-point: x1 = x0 + s * d.

;; First we give a rule/conditions to tell whether a step got is good.
;; Armijo, Wolfe rule and Strong Wolfe rule are widely used.
;; The two conditions belows are basic conditions/rule to construct Armijo, Wolfe and Strong Wolfe rules.

;; get-step function
(define (get-step f grad epsilon c1 c2)                     ;get-step
  ;; epsilon: minimun of distance of two neighbor points
  (lambda (x d default-step)
    (define (y s)
      (f (vector-add x (scalar-*-vector s d))))
    (define (dy s)
      (vector-dot d (grad (vector-add x (scalar-*-vector s d)))))
    (if default-step
        (line-search-algorithm y dy default-step 'any c1 c2)   ;line-search-algorithm
        (let ([min-step (* 10 (/ epsilon (vector-norm/inf d)))])
          (line-search-algorithm y dy #f min-step c1 c2)))))

                                        ; Armijo condition (ac?)
(define (armijo-condition? c1)
  (lambda (y0 y1 dy0 s)
    ;;y0 = y(0), others like
    (<= y1 (+ y0 (* c1 s dy0)))))
                                        ; curvature condition (cc?)
(define (curvature-condition? c2)
  (lambda (dy0 dys)
    ;;dy0 = dy(0), dys = dy(s)
    (<= (abs dys) (* c2 (abs dy0)))))

;; --- line search algorithm
;; we use Strone Wolfe rule(i.e. ac? + cc?)
(define (line-search-algorithm y dy default-step min-step c1 c2)
  ;; assume: 0 < c1 < c2 <1, dy(0) < 0
  ;; if given default-step, use default-step, else use min-step to construct step-size
  ;; return: some s satisfing strong-wolfe-rule.
  (let ([y0 (y 0)] [dy0 (dy 0)])
    (define (sdc? s s0)                     ;sdc?(sufficient-desceant-condition, not only ac?)
      (lambda (ys ys0)
        (and ((armijo-condition? c1) y0 ys dy0 s)
             (<= ys ys0))))
    (define (cc? s)                         ;cc?
      (lambda (dys)
        ((curvature-condition? c2) dy0 dys)))
    (define step-size                       ;step-size
      (if default-step default-step
          (let ([ddy0 (/ (- (dy min-step) dy0) min-step)])
            (if (positive? ddy0) (- (/ dy0 ddy0)) min-step))))
    (define (next-step s) (+ s step-size))  ;next-step
                                        ;zoom
    (define (zoom current bound)
      ;; current satisfies sdc?, but not cc?;
      ;; make sure that there must be one s between current and bound.
      (lambda (yc dyc yb)
        (let* ([s (inner-point current bound yc dyc yb)]
               [ys (y s)]
               [dys (dy s)])
          ;;(display "  **zoom**  ")
          (if ((sdc? s current) ys yc)
              (cond [((cc? s) dys) s] ;ok
                    [(negative? (* dys (- bound current)))
                     ((zoom s bound) ys dys yb)]
                    (else
                     ((zoom s current) ys dys yc)))
              ((zoom current s) yc dyc ys)))))
    (define (inner-point c b yc dyc yb)           ; inner-point
      (let ([aux (/ (+ b c) 2 (- 1 (/ (- yb yc) (- b c) dyc)))])
        (if (or (nan? aux) (<= (* (- aux c) (- b aux)) 0))
            (/ (+ b c) 2) aux)))
    ;; main part: loop to get an increasing seq: s0 = 0 < s1 < s2 < ...,
    ;;until one can satisfy strong-wolfe-rule, one exception is s_i may be too large.
    (if (and ((sdc? 0 0) y0 y0) ((cc? 0) dy0))
        0 ;ok
        (let loop ([s0 0] [ys0 y0] [dys0 dy0]
                   [s1 step-size] [ys1 (y step-size)])
          ;;require: y'(s0) < 0 and s0 < s1
          ;;(display "  --loop--  ")
          (if ((sdc? s1 s0) ys1 ys0)
              (let ([dys1 (dy s1)])
                (cond [((cc? s1) dys1) s1] ;ok
                      [(negative? dys1) (let ([next-s1 (next-step s1)])
                                          (loop s1 ys1 dys1 next-s1 (y next-s1)))]
                      (else ((zoom s1 s0) ys1 dys1 ys0)))) ;ok
              ((zoom s0 s1) ys0 dys0 ys1))))) ;ok
  )


;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;   (B) find-direction
;;;;;;;;;;;;;;;;;;;;;;;;;;;
#|  There are some different methods to find-direction:

(B1) (SD) Steepest-Descent,  whose direction is opposite of gradiant's direction,
(B2) (CG) Conjugate-Gradiant method
(B3) (Subspace) Subspace method
(B4) (Newton, Quasi-Newton) Newton method and Quasi-Newton methods, which are approximations of Newton method.

|#
                                        ; descent-direction?
(define (descent-direction? d g epsilon)
  (< (/ (vector-dot d g) (vector-norm d) (vector-norm g)) (- epsilon)))
                                        ; next => x + s * d
(define (next x s d)
  (vector-add x (scalar-*-vector s d)))


;; (B1) (SD) Steepest-Descent method 
(define (SD g1)
  ;; return: d1 = -g1
  (scalar-*-vector -1 g1))

;; (B2) (CG) Conjugate Gradient methods
(define (CG d0 g1 g0 beta-function)
  ;;return: d1
  ;;beta-function: the special method to be used
  ;; d1 may not be descent.
  (vector-add (scalar-*-vector -1 g1)
              (scalar-*-vector (beta-function g1 g0) d0)))

;; The beta-functions are below:
;; (a)FR
(define (FR/beta g1 g0)
  (/ (vector-dot g1 g1)
     (vector-dot g0 g0)))
;; (b)PR
(define (PR/beta g1 g0)
  (/ (vector-dot g1 (vector-minus g1 g0))
     (vector-dot g0 g0)))
;; (c)PR+
(define (PR+/beta g1 g0)
  (max (PR/beta g1 g0) 0))


;; (B3) (Subspace) Subspace method
;; this method give direction with step-length at the same time(like Newton method)
(define (subspace grad ng0 ug0 nd0 ud0 x1 ng1 ug1 epsilon)
  ;; x0's info: g0;   d0=x1-x0;    x1's info: g1;
  ;; nv: norm of v, uv: unit-vector of v;
  ;; return: (cons nd1 ud1)
  (let* ([dots (map vector-dot
                    ;;unit-vec d0g0  d0g1 g0g1
                    (list       ud0  ud0  ug0)
                    (list       ug0  ug1  ug1))]
         [s epsilon] [xs (next x1 (- s) ug1)]
         [gs (grad xs)] [ngs (vector-norm gs)] [ugs (scalar-*-vector (/ 1 ngs) gs)]
         [g1/d0 (/ ng1 nd0)] [g0/g1 (/ ng0 ng1)] [gs/g1 (/ ngs ng1)] [g1/ds (/ ng1 s)]
         [d0Gd0 (* g1/d0 (- (list-ref dots 1) (* g0/g1 (list-ref dots 0))))]
         [d0Gg1 (* g1/d0 (- 1 (* g0/g1 (list-ref dots 2))))]
         [g1Gg1 (* g1/ds (- 1 (* gs/g1 (vector-dot ug1 ugs))))])
    (if (and (positive? g1Gg1) (positive? (- (* d0Gd0 g1Gg1) (expt d0Gg1 2))));;确保G正定
        (let* ([beta (/ (- (* (list-ref dots 1) g1Gg1) d0Gg1)
                        (- (* (list-ref dots 1) d0Gg1) d0Gd0))]
               [pre-s (* ng1 (/ (- 1 (* beta (list-ref dots 1)))
                                (+ g1Gg1 (* -2 beta d0Gg1) (* beta beta d0Gd0))))]
               [dir (scalar-*-vector pre-s (vector-minus (scalar-*-vector beta ud0) ug1))]
               [ndir (vector-norm dir)] [udir (scalar-*-vector (/ 1 ndir) dir)])
          (if (descent-direction? udir ug1 epsilon)
              (cons ndir udir) #f))
        #f)))

;; (B4) (Quasi-Newton-SR1) Symmetric-Rank-1 method
;; aux function: to get matrix: v * v^T
(define (column-*-row v)
  (vector-map (lambda (ele) (scalar-*-vector ele v)) v))

(define (Quasi-Newton-SR1 x1 g1 x0 g0 H0 B0)
  ;;return: (list d1 H1 B1)
  ;; d1 may not be descent.
  (let* ([s (vector-minus x1 x0)]
         [y (vector-minus g1 g0)]
         [s-Hy (vector-minus s (matrix-*-vector H0 y))]
         [y-Hs (vector-minus y (matrix-*-vector B0 s))]
         [c1 (vector-dot s-Hy y)]
         [c2 (vector-dot y-Hs s)]
         [H1 (matrix-add H0 (scalar-*-matrix (/ 1 c1) (column-*-row s-Hy)))];column-*-row
         [B1 (matrix-add B0 (scalar-*-matrix (/ 1 c2) (column-*-row y-Hs)))])
    (list (scalar-*-vector -1 (matrix-*-vector H1 g1)); d1 = -H1 * g1
          H1 B1)))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Line-search algorithm + find-direction methods
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;; Now we have all needed, and can complete the job!

;; (SD) Steepest Descent method
;;; stream version
(define (SD-method-stream f grad x-init epsilon c1 c2)
  (define (SD-direction x) (SD (grad x)))
  (define (SD-step x d) ((get-step f grad epsilon c1 c2) x d #f))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let loop-stream ([x x-init])
    (stream-cons x
                 (let* ([d (SD-direction x)]
                        [s (SD-step x d)])
                   (loop-stream (next x s d))))
    ))
;;; ordinary version
(define (SD-method f grad x-init epsilon c1 c2)
  (define (SD-direction x) (SD (grad x)))
  (define (SD-step x d) ((get-step f grad epsilon c1 c2) x d #f))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let loop ([x x-init])
    (let* ([d (SD-direction x)]
           [s (SD-step x d)]
           [next-x (next x s d)])
      ;;(display (f next-x)) (newline)
      (if (< (vector-norm/inf (grad next-x)) epsilon) ;;终止条件
          next-x ;=>
          (loop next-x)))))

;; (Subspace) Subspace method
;; stream version
(define (Subspace/with-SD-method-stream f grad x-init epsilon c1 c2)
  (define (SD-direction g) (scalar-*-vector (/ -1 (vector-norm g)) g));;unit-dir
  (define step-function (get-step f grad epsilon c1 c2))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let* ([x0 x-init] [g0 (grad x0)] [ng0 (vector-norm g0)] [ug0 (scalar-*-vector (/ 1 ng0) g0)]
         [ud0 (SD-direction g0)] [s0 (step-function x0 ud0 #f)] [x1 (next x0 s0 ud0)]
         [g1 (grad x1)] [ng1 (vector-norm g1)] [ug1 (scalar-*-vector (/ 1 ng1) g1)])
    (let loop-stream ([x0 x0] [ng0 ng0] [ug0 ug0] [s0 s0] [ud0 ud0]
                      [x1 x1] [ng1 ng1] [ug1 ug1])
      (stream-cons x0
                   (let ([sub (subspace grad ng0 ug0 s0 ud0 x1 ng1 ug1 epsilon)])
                     (if sub
                         (let* ([s1 (step-function x1 (cdr sub) (car sub))]
                                [x2 (next x1 s1 (cdr sub))]
                                [g2 (grad x2)] [ng2 (vector-norm g2)] [ug2 (scalar-*-vector (/ 1 ng2) g2)])
                           (loop-stream x1 ng1 ug1 s1 (cdr sub) x2 ng2 ug2))
                         (let* ([d1-SD (SD-direction g1)] [s1-SD (step-function x1 d1-SD #f)]
                                [x2-SD (next x1 s1-SD d1-SD)] [g2-SD (grad x2-SD)]
                                [ng2-SD (vector-norm g2-SD)] [ug2-SD (scalar-*-vector (/ 1 ng2-SD) g2-SD)])
                           (display "  \n**in <Sub>, SD is used**\n   ")
                           (loop-stream x1 ng1 ug1 s1-SD d1-SD x2-SD ng2-SD ug2-SD)))))
      )))
;; ordinary version
(define (Subspace/with-SD-method f grad x-init epsilon c1 c2)
  (define (SD-direction g) (scalar-*-vector (/ -1 (vector-norm g)) g));;unit-dir
  (define step-function (get-step f grad epsilon c1 c2))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let* ([x0 x-init] [g0 (grad x0)] [ng0 (vector-norm g0)] [ug0 (scalar-*-vector (/ 1 ng0) g0)]
         [ud0 (SD-direction g0)] [s0 (step-function x0 ud0 #f)] [x1 (next x0 s0 ud0)]
         [g1 (grad x1)] [ng1 (vector-norm g1)] [ug1 (scalar-*-vector (/ 1 ng1) g1)])
    (let loop ([x0 x0] [ng0 ng0] [ug0 ug0] [s0 s0] [ud0 ud0]
               [x1 x1] [ng1 ng1] [ug1 ug1])
      (if (< ng1 epsilon) x1 ;ok
          (let ([sub (subspace grad ng0 ug0 s0 ud0 x1 ng1 ug1 epsilon)])
            (if sub
                (let* ([s1 (step-function x1 (cdr sub) (car sub))]
                       [x2 (next x1 s1 (cdr sub))]
                       [g2 (grad x2)] [ng2 (vector-norm g2)] [ug2 (scalar-*-vector (/ 1 ng2) g2)])
                  (loop x1 ng1 ug1 s1 (cdr sub) x2 ng2 ug2))
                (let* ([d1-SD (SD-direction g1)] [s1-SD (step-function x1 d1-SD #f)]
                       [x2-SD (next x1 s1-SD d1-SD)] [g2-SD (grad x2-SD)]
                       [ng2-SD (vector-norm g2-SD)] [ug2-SD (scalar-*-vector (/ 1 ng2-SD) g2-SD)])
                  (display "  \n**in <Sub>, SD is used**\n   ")
                  (loop x1 ng1 ug1 s1-SD d1-SD x2-SD ng2-SD ug2-SD)))))
      )))


#|;; (Newton, Quasi-Newton)
;; SR1 method
init-vals:  x0, H0=I, B0=I, d0=-grad(x0),
          (x0 s0 d0 H0 B0) => (x1 s1 d1 H1 B1),
where:    x1 = x0 + s0 * d0,
          s1: use line-search-algorithm,
          (d1,H1,B1): use Quasi-Newton-SR1 method.
|#
;; stream-version
(define (Quasi-Newton-SR1-method-stream f grad x-init epsilon c1 c2)
  ;; require: 0 < c1 < c2 < 1
  ;; return: stream of x0=x-init, x1, x2, ...
  (define step-function (get-step f grad epsilon c1 c2))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let* ([n (vector-length x-init)]
         [d-init (SD (grad x-init))];initial d is used by SD method
         [id (identity n)])
    (let loop-stream ([x0 x-init] [g0 (grad x-init)]
                      [d0 d-init] [s0 (step-function x-init d-init 1.0)]
                      [H0 id] [B0 id])
      (stream-cons x0
                   (let* ([x1 (next x0 s0 d0)]
                          [g1 (grad x1)]
                          [dHB (Quasi-Newton-SR1 x1 x0 g1 (grad x0) H0 B0)];Quasi-Newton-SR1
                          [pre-d1 (car dHB)])
                     (if (descent-direction? pre-d1 g1 epsilon)
                         (apply loop-stream
                                (append (list x1 g1 pre-d1 (step-function x1 pre-d1 1.0)) (cdr dHB)))
                         (let ([d1-SD (SD g1)])
                           (apply loop-stream
                                  (list x1 g1 d1-SD (step-function x1 d1-SD #f) id id)))))))
    ))
;; ordinary-version
(define (Quasi-Newton-SR1-method f grad x-init epsilon c1 c2)
  ;; require: 0 < c1 < c2 < 1
  (define step-function (get-step f grad epsilon c1 c2))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let* ([n (vector-length x-init)]
         [d-init (SD (grad x-init))];initial d is used by SD method
         [id (identity n)])
    (let loop ([x0 x-init] [g0 (grad x-init)]
               [d0 d-init] [s0 (step-function x-init d-init 1.0)]
               [H0 id] [B0 id])
      (let* ([x1 (next x0 s0 d0)]
             [g1 (grad x1)]
             [dHB (Quasi-Newton-SR1 x1 x0 (grad x1) (grad x0) H0 B0)]
             [pre-d1 (car dHB)])
        (if (< (vector-norm/inf x1) epsilon) x1   ;ok
            (if (descent-direction? pre-d1 g1 epsilon)
                (apply loop (append (list x1 g1 pre-d1 (step-function x1 pre-d1 1.0)) (cdr dHB)))
                (let ([d1-SD (SD g1)])
                  (display "\n**in <Quasi>:use SD**")
                  (apply loop (list x1 g1 d1-SD (step-function x1 d1-SD #f) id id)))))))
    ))


;;---- (CG) Conjugate Gradient method
;; stream version
(define (CG/with-SD-method-stream beta/fun)
  ;; beta/fun can be FR/beta, PR/beta, PR+/beta, ..
  (lambda (f grad x-init epsilon c1 c2)
    ;; require: 0 < c1 < c2 < 1/2
    (define (CG/SD-direction x)
      (lambda (g g0 d0)
        (let ([d (CG d0 g g0 beta/fun)])
          (if (descent-direction? d g epsilon) d
              (begin (display "\n**in <CG>:use SD**")
                     (SD g))))))
    (define step-function (get-step f grad epsilon c1 c2))
    (assert (and (< 0 c1) (< c1 c2) (< c2 1/2)))
    (let* ([x0 x-init]
           [g0 (grad x-init)]
           [d0 (SD g0)]
           [s0 (step-function x0 d0 #f)]
           [x1 (next x0 s0 d0)]
           [g1 (grad x1)])
      (let loop-stream ([x0 x0] [g0 g0] [d0 d0]
                        [x1 x1] [g1 g1])
        (stream-cons x0
                     (let* ([d1 ((CG/SD-direction x0) g1 g0 d0)]
                            [s1 (step-function x1 d1 #f)]
                            [x2 (next x1 s1 d1)]
                            [g2 (grad x2)])
                       (loop-stream x1 g1 d1 x2 g2)))
        ))))
;; ordinary version
(define (CG/with-SD-method beta/fun)
  ;; beta/fun can be FR/beta, PR/beta, PR+/beta, ..
  (lambda (f grad x-init epsilon c1 c2)
    ;; require: 0 < c1 < c2 < 1/2
    (define (CG/SD-direction x)
      (lambda (g g0 d0)
        (let ([d (CG d0 g g0 beta/fun)])
          (if (descent-direction? d g epsilon) d
              (begin (display "\n**in <CG>:use SD**")
                     (SD g))))))
    (define step-function (get-step f grad epsilon c1 c2))
    (assert (and (< 0 c1) (< c1 c2) (< c2 1/2)))
    (let* ([x0 x-init]
           [g0 (grad x-init)]
           [d0 (SD g0)]
           [s0 (step-function x0 d0 #f)]
           [x1 (next x0 s0 d0)]
           [g1 (grad x1)])
      (let loop ([x0 x0] [g0 g0] [d0 d0] [s0 s0] [x1 x1] [g1 g1])
        (if (< (vector-norm/inf x1) epsilon)
            x1
            (let* ([d1 ((CG/SD-direction x0) g1 g0 d0)]
                   [s1 (step-function x1 d1 #f)]
                   [x2 (next x1 s1 d1)]
                   [g2 (grad x2)])
              (loop x1 g1 d1 s1 x2 g2)))
        ))))

